#include "zf_common_headfile.h"
//25.2
extern EulerAngleTypedef SystemAttitudeRate;        /////��̬���ٶ�
extern EulerAngleTypedef       SystemAttitude;            /////��̬��

//����ʽPID�ٶȻ���������
#define a0e (Speed_P+Speed_I+Speed_D)
#define a1e (Speed_P+2*Speed_D)
#define a2e Speed_D

#define Ramp_DownSpeed 250                //�����ٶ�

float AngleAim = 0;
//�ٶȻ���������
//static float EncoderSpeed_Out = 0, LastEncoderSpeed_Out = 0;

float Speed_OutAngle = 0, Speed_Sum = 0, Speed_OutAngleNew = 0, Speed_ErrorAccumulated = 0,
        ErrorAccumulated_I = 0.00001;
int32 Speed_RunOutControlNum = 0;
float speed_LastAngle = 0;

float Encoder_Integral = 0, LastSpeed = 0;
//�ǶȻ���������
float RealAngle;            //�Ƕ�
float Balance_Speed_Out;            //ƽ������ٶ�

//���ٶȻ���������
//static float Gyro_Error_Count;
float Gyro_Speed_Out = 0, Gyro_Error = 0, Gyro_Error_Last = 0;

//ת�򻷲�������
float TurnZero_Out = 0;
float TurnSpeed_Out = 0;

//Ԫ���ٶ�����
float Speed_Aim_Buff = 0;

smoothness_data_t  smoothness_data=
{
        .flag = 0,
        .encoder.unit = 150,
        .encoder.current = 0,
        .encoder.target = 0,
        .error.fusion = 0,
        .error.current = 0,
        .error.new = 0,
};

//ֱ��PID����***************************************
float ZLKp = 0,ZLKi = 0,ZLKd = 0;
#define D0 (ZLKp + ZLKi + ZLKd)
#define D1 (ZLKp + 2*ZLKd)
#define D2 ZLKd
float seep_L_out = 0,seep_R_out = 0;


//************************************************



/*
 * ƫ��ƽ������
 *
 */
void smoothness_init(float current_error, float new_error)
{
    smoothness_data.error.current = current_error;
    smoothness_data.error.new = new_error;

    smoothness_data.encoder.current = 0;
    smoothness_data.encoder.target = fabs(smoothness_data.error.new - smoothness_data.error.current) * smoothness_data.encoder.unit;

    smoothness_data.flag = 1;
}

float smoothness(void)
{
    float current_percentage;
    float new_percentage;

    new_percentage = smoothness_data.encoder.current / smoothness_data.encoder.target;
    current_percentage = 1.0 - new_percentage;
    smoothness_data.error.fusion = smoothness_data.error.current *  current_percentage + smoothness_data.error.new * new_percentage;

    if(smoothness_data.encoder.current >= smoothness_data.encoder.target)
    {
        smoothness_data.flag = 0;
        clean_smoothness_data();
    }
    return smoothness_data.error.fusion;
}

char get_smoothness_state(void)
{
    return smoothness_data.flag;
}

void clean_smoothness_data(void)
{
    smoothness_data.error.current = 0;
    smoothness_data.error.new = 0;

    smoothness_data.encoder.current = 0;
    smoothness_data.encoder.target = 0;
}
extern AttitudeDatatypedef         Gyro;

/*
 *  @�������ã�19�����ֶ��ת��PD
 *  @�������ߣ�����
 *  @�������֣�float PID_roll_Servo (void)
 *  @������������
 *  @��������ֵ����
 */

float PID_roll_Servo (void)
{
    float error = 0;
    static float error_last=0;
    static float servo_Pid = 0;
//    float kp,kd;
//    float a = 1;
//    float fbasf_angle = fabsf(angle_value);
//    /* ���Էֶ�kp */
//    kp = Servo_P * (0.85 + fbasf_angle / 60);
//    /* ���Էֶ�kd */
//    kd = Servo_D * (1.30 - fbasf_angle / 60);

    error = angle_value;

    servo_Pid = Servo_P * error + Servo_D * (error - error_last);
    error_last = error;
    servo_Pid=servo_Pid>(Servo_Right)?(Servo_Right):servo_Pid;// ����Ƕ�����
    servo_Pid=servo_Pid<(Servo_Left)?(Servo_Left):servo_Pid;// ����Ƕ�����
    return servo_Pid;
}


/*  @����˵����19�������ٶȻ�����ʽPID��
 *  @�������ߣ�����
 *  @��������(float speed)��Ŀ���ٶ�
 *  @��������ֵ�������ۻ������
 *
 * */
float errrrr = 0;
float silun_PID_speed_left(float speed)
{
    static float error_speed0 = 0, error_speed1 = 0, error_speed2 = 0,error_speed=0,error_speed0_old=0,add_speed_old[4];
    static float speed_Pid1 = 0;
    float add_speed = 0;//�����ٶ�
    static float speederrorMax1=0;
    /* ������ */
//    float x = 0;
//    float abs_error = fabsf(angle_value);
//    if(abs_error>20)
//        x = Speed_Aim;
//    else
//        x = Speed_Aim*(2-abs_error/20);
    errrrr = error_speed0 = Speed_Aim - speed;


    add_speed = a0e * error_speed0 - a1e * error_speed1 + a2e * error_speed2;

    error_speed2 = error_speed1;
    error_speed1 = error_speed0;
    speed_Pid1 += add_speed;
    if(speed_Pid1>999)speed_Pid1=999;
    else if(speed_Pid1<-999)speed_Pid1=-999;
    return speed_Pid1;
}

/*  @����˵����19�������ٶȻ�����ʽPID�һ�
 *  @�������ߣ�����
 *  @��������(float speed)��Ŀ���ٶ�
 *  @��������ֵ�������ۻ������
 *
 * */
float silun_PID_speed_right(float speed)
{
    static float error_speed0 = 0, error_speed1 = 0, error_speed2 = 0,error_speed=0,error_speed0_old=0,add_speed_old[4];
    static float speed_Pid1 = 0;
    float add_speed = 0;//�����ٶ�
    static float speederrorMax1=0;
    /* ������ */
    error_speed0 = Speed_Aim - speed;

    add_speed = a0e * error_speed0 - a1e * error_speed1 + a2e * error_speed2;

    error_speed2 = error_speed1;
    error_speed1 = error_speed0;
    speed_Pid1 += add_speed;
    if(speed_Pid1>999) speed_Pid1=999;
    else if(speed_Pid1<-999) speed_Pid1=-999;
    return speed_Pid1;
}

/*
 * ���ת��
 * Servo_P 72
 * Servo_D 0.55
 *
 */

PID PID_turn_speed;

float PID_compute_turn_speed (void)
{
    PID_turn_speed.setpoint = (float) fabs(SystemAttitude.Roll - zero.out);

    PID_turn_speed.output = PID_turn_speed.q1 * PID_turn_speed.setpoint + PID_turn_speed.q3 * (PID_turn_speed.setpoint - PID_turn_speed.last_error);
    PID_turn_speed.last_error = PID_turn_speed.setpoint;

    PID_turn_speed.output = PID_turn_speed.output > 10 ? 10 : PID_turn_speed.output;
    PID_turn_speed.output = PID_turn_speed.output < -10 ? -10 : PID_turn_speed.output;
    return PID_turn_speed.output;
}

float PID_pitch_speed (float speed)
{
    static float error_speed0 = 0, error_speed1 = 0, error_speed2 = 0,error_speed=0,error_speed0_old=0,add_speed_old[4];
    static float speed_Pid = 0,yansjs=0,TurnSpeed_Out_Old=0;
    float add_speed = 0;
    static float speederrorMax=0;
//    if(Speed_Aim)
//    if(PID_flag==1)
//    {
//        PID_flag=0;
//        speed_Pid = 0;
//        error_speed2=0;
//        error_speed1=0;
//    }

//    if(zhi_flag==0&&Ramp_Flag==0&&Ring_Flag==0&&duanflag==0&&bmx_flag==0&&straight_rush_flag==0){//Ԫ�ر仯�ٶȴ���
//        if(TurnSpeed_Out<0)
     error_speed = 50-speed+TurnSpeed_Out;
//        else
//            error_speed = 50-speed+TurnSpeed_Out/12;//�ٶ�155��ʱ�����Ϊ10

//    }
//    else if(zhi_flag==1)
//   {
//       if(TurnSpeed_Out>0)
//               error_speed = Speed_Aim_Buff-speed+TurnSpeed_Out;
//           else
//               error_speed = Speed_Aim_Buff-speed+TurnSpeed_Out/8;
//   }
//    else
//    {
//    if(TurnSpeed_Out<0)
//            error_speed = Speed_Aim_Buff-speed+TurnSpeed_Out;
//        else
//            error_speed = Speed_Aim_Buff-speed+TurnSpeed_Out/12;
//    }
    speederrorMax=speederrorMax<50?speederrorMax+0.3:50;
//    error_speed0 =(error_speed)>speederrorMax?speederrorMax:(error_speed);//�ٶ�ƽ������
    if(zhi_flag==0)
        error_speed0=error_speed>0?((error_speed)>speederrorMax?speederrorMax:(error_speed)):((error_speed)<-speederrorMax?-speederrorMax:(error_speed));
    else
        error_speed0 =(error_speed)>speederrorMax?speederrorMax:(error_speed);
    VOFA_row[15]=error_speed0;

    error_speed0_old=error_speed0;

    if(fabs(speed-Speed_Aim) < 0)
        add_speed = a0e * error_speed0 - a1e * error_speed1 + a2e * error_speed2;
    else
        add_speed = (Speed_P+Speed_D) * error_speed0 - a1e * error_speed1 + a2e * error_speed2;
//    add_speed=add_speed_old[0]*0.4+add_speed_old[1]*0.3+add_speed_old[2]*0.2+add_speed_old[3]*0.1;
//
//     add_speed_old[3]=add_speed_old[2];
//     add_speed_old[2]=add_speed_old[1];
//     add_speed_old[1]=add_speed_old[0];
    error_speed2 = error_speed1;
    error_speed1 = error_speed0;
    speed_Pid += add_speed;
    if(speed_Pid>15000)speed_Pid=15000;
//    else if(speed_Pid<-15000)speed_Pid=-15000;

    return speed_Pid;
}

float PID_pitch_speed1 (float speed)
{
    static float error_speed0 = 0, error_speed1 = 0, error_speed2 = 0,error_speed=0,error_speed0_old=0,add_speed_old[4];
    static float speed_Pid1 = 0;
    float add_speed = 0;
    static float speederrorMax1=0;

//    if(PID_flag==1)
//    {
//        PID_flag=0;
//        speed_Pid1 = 0;
//        error_speed2=0;
//        error_speed1=0;
//    }

    if(zhi_flag==0&&Ramp_Flag==0&&Ring_Flag==0&&duanflag==0&&bmx_flag==0){//Ԫ�ر仯�ٶȴ���
    if(TurnSpeed_Out>0)
        error_speed = Speed_Aim-speed-TurnSpeed_Out;
    else
        error_speed = Speed_Aim-speed-TurnSpeed_Out/12;
    }
    else if(zhi_flag==1)
    {
        if(TurnSpeed_Out>0)
                error_speed = Speed_Aim_Buff-speed-TurnSpeed_Out;
            else
                error_speed = Speed_Aim_Buff-speed-TurnSpeed_Out/8;
    }
    else{
        if(TurnSpeed_Out>0)
                error_speed = Speed_Aim_Buff-speed-TurnSpeed_Out;
            else
                error_speed = Speed_Aim_Buff-speed-TurnSpeed_Out/12;
    }

    speederrorMax1=speederrorMax1<50?speederrorMax1+0.3:50;
//    error_speed0 =(error_speed)>speederrorMax1?speederrorMax1:(error_speed);//�ٶ�ƽ������
    if(zhi_flag==0)
        error_speed0=error_speed>0?((error_speed)>speederrorMax1?speederrorMax1:(error_speed)):((error_speed)<-speederrorMax1?-speederrorMax1:(error_speed));
    else
        error_speed0 =(error_speed)>speederrorMax1?speederrorMax1:(error_speed);
      error_speed0_old=error_speed0;
      if(fabs(speed-Speed_Aim) < 0)
          add_speed = a0e * error_speed0 - a1e * error_speed1 + a2e * error_speed2;
      else
          add_speed= (Speed_P+Speed_D) * error_speed0 - a1e * error_speed1 + a2e * error_speed2;


//      add_speed=add_speed_old[0]*0.4+add_speed_old[1]*0.3+add_speed_old[2]*0.2+add_speed_old[3]*0.1;
//
//          add_speed_old[3]=add_speed_old[2];
//          add_speed_old[2]=add_speed_old[1];
//          add_speed_old[1]=add_speed_old[0];

    error_speed2 = error_speed1;
    error_speed1 = error_speed0;

    speed_Pid1 += add_speed;
//    VOFA_row[18]=speed_Pid1;
    if(speed_Pid1>15000)speed_Pid1=15000;
//    else if(speed_Pid1<-15000)speed_Pid1=-15000;
    return speed_Pid1;
}


float zero_adjust_pitch_speed = 0.25;
float zero_p_r = 1.0;
float zero_p_l = 1.0;

ZERO_t zero;
void zero_control (void)
{
    zero.setpoint = angle_value;

    zero.out = zero.p * angle_value + zero.d * angle_value_Error;
    if (Balance_Test == 1)
    {
        zero.out = 0;
    }
    zero.out = zero.out > zero.max ? zero.max : zero.out;
    zero.out = zero.out < (0 - zero.max) ? (0 - zero.max) : zero.out;
}

/*
 * Balance_P 150
 * Balance_I 29
 * Balance_D 16
 *
 * Speed_Aim = 4������Ҵ��ϵ��Ϊ0.00076���������ϵ��Ϊ0.0006
 */

LQR_t LQR;
float LQR_roll_control (float roll, float rollrate, float encoder)
{
    static float roll_error0 = 0, roll_error1 = 0, roll_error2 = 0;
    static float roll_Pid = 0;
    static float zero_adjust = 0;
    float add_roll = 0;
//    zero_adjust =
//            roll_Servo_Out > 0 ?
//                    0.00076f * roll_Servo_Out * Encoder_RightSpeed * zero_adjust_pitch_speed :
//                    0.0006 * roll_Servo_Out * Encoder_RightSpeed * zero_adjust_pitch_speed;
    zero_adjust = zero.out;

    roll_error0 = roll + zero_adjust + Balance_VER;//Balance_VER 0.3
    roll_error1 = rollrate;
    roll_error2 = encoder;

    if (fabs(roll_error0) < 5)
    {
        add_roll = LQR.q1 * roll_error0 + LQR.q2 * roll_error1 + LQR.q3 * roll_error2;
    }
    else
    {
        add_roll = 0;
    }
    roll_Pid = add_roll;
    return roll_Pid;
}

/*----------------2022.5.1----------------*/
/*
 * �����ֽ��ٶȻ�
 *  PD����
 */
PID PID_roll_velocity;

void PID_compute_roll_velocity (void)
{
    float error;

    error = PID_roll_velocity.setpoint - Gyro.Xdata;
    PID_roll_velocity.integral += error;

    if (fabs(SystemAttitude.Roll) < 5.0)
        PID_roll_velocity.output = PID_roll_velocity.q1 * error
                + PID_roll_velocity.q3 * (error - PID_roll_velocity.last_error);
    else
        PID_roll_velocity.output = 0;
    PID_roll_velocity.last_error = error;
}

/*----------------2022.5.1----------------*/
/*
 * �����ֽǶȻ�
 *  ��P����
 */
PID PID_roll;
void PID_compute_roll (void)
{
    float error;

    error = SystemAttitude.Roll + PID_roll.setpoint + Balance_VER - 1.4;

    PID_roll.output = PID_roll.q1 * error;

    PID_roll_velocity.setpoint = PID_roll.output;
}

/*----------------2022.5.1----------------*/
/*
 * �������ٶȻ�
 * PI����
 */
PID PID_flywheel;
void PID_compute_flywheel (void)
{
    float error;

    error = Encoder_LeftSpeed;
    PID_roll_velocity.integral += error;

    PID_flywheel.output = PID_flywheel.q1 * error + PID_flywheel.q2 * PID_roll_velocity.integral;

    PID_flywheel.output = PID_flywheel.output > 5.0 ? 5.0 : PID_flywheel.output;
    PID_flywheel.output = PID_flywheel.output < -5.0 ? -5.0 : PID_flywheel.output;

    PID_roll.setpoint = PID_flywheel.output;
}


//���Ե�����ͷѭ�������ö�̬�ֶ�PID
void Turn_PID(float servo_int)
{
     float Kp = 0,Kd = 0,Quadratic_Encoder_RawSpeedSum =0;
     static TurnSpeed_Out_net[4],TurnSpeed_Out_net_new=0;
     static kdOut=0,kdOutLast=0,shucuOutLast=0;
     static float kdXishu = 0;

     /* һ���˲��� */
     TurnSpeed_Out_net_new = tureKp*servo_int + tureKd*(servo_int-shucuOutLast);
     shucuOutLast = servo_int;


         TurnSpeed_Out_net[3] = TurnSpeed_Out_net[2];
         TurnSpeed_Out_net[2] = TurnSpeed_Out_net[1];
         TurnSpeed_Out_net[1] = TurnSpeed_Out_net[0];
         TurnSpeed_Out_net[0] = TurnSpeed_Out_net_new;

         TurnSpeed_Out = TurnSpeed_Out_net[0]*0.4+TurnSpeed_Out_net[1]*0.3+TurnSpeed_Out_net[2]*0.2+TurnSpeed_Out_net[3]*0.1;

//               TurnSpeed_Out=TurnSpeed_Out_net[0];
         TurnSpeed_Out = TurnSpeed_Out > 999 ? 999 : TurnSpeed_Out;
         TurnSpeed_Out = TurnSpeed_Out < -999 ? -999 : TurnSpeed_Out;
//               TurnSpeed_Out=(Diff_P * angle_value + Diff_D * derror -Servo_I*Balance_Dir_Gyro_Now/10 );//
//                  else
//               0   TurnSpeed_Out=(Diff_P * angle_value- Servo_I*Balance_Dir_Gyro_Now/10 );

}




void Turn_PID1(void)
{
     float Kp = 0,Kd = 0,Quadratic_Encoder_RawSpeedSum =0;
     static float TurnSpeed_Out_net[4],TurnSpeed_Out_net_new=0,Diff_P1=0,Diff_D1=0,X_Deviation_old=0;

//     static float last_error_d,last_derror_d,derror=0;
        if(Encoder_RawSpeedSum<90)
        {
              Diff_P1 = (float)LQR.q1/130.0*((XY_Deviation*XY_Deviation)/8100+0.9);
              Diff_D1 = (float)LQR.q3/100.0;
        }
        else if(Encoder_RawSpeedSum<100)
        {
              Diff_P1 = (float)LQR.q1/120.0*((XY_Deviation*XY_Deviation)/8100+0.9);
              Diff_D1 = (float)LQR.q3/100.0;
        }
        else if(Encoder_RawSpeedSum>110)
        {
              Diff_P1 = (float)LQR.q1/110.0*((XY_Deviation*XY_Deviation)/8100+0.9);
              Diff_D1 = (float)LQR.q3/100.0;
        }
        else if(Encoder_RawSpeedSum<120)
        {
              Diff_P1 = (float)LQR.q1/105.0*((XY_Deviation*XY_Deviation)/8100+0.9);
              Diff_D1 = (float)LQR.q3/100.0;
        }
        else
        {
              Diff_P1 = (float)LQR.q1/100.0*((XY_Deviation*XY_Deviation)/8100+0.9);
              Diff_D1 = (float)LQR.q3/100.0;
        }


        if(Cross_Flag==1)
            TurnSpeed_Out_net_new=-(Diff_P1 * X_Deviation + Diff_D1 * (X_Deviation-X_Deviation_old) -LQR.q2*Balance_Dir_Gyro_Now/100 );//
        else
            TurnSpeed_Out_net_new=-(Diff_P1 * XY_Deviation + Diff_D1 * (XY_Deviation_Error) -LQR.q2*Balance_Dir_Gyro_Now/100 );//

               X_Deviation_old=X_Deviation;

              TurnSpeed_Out_net[3]=TurnSpeed_Out_net[2];
              TurnSpeed_Out_net[2]=TurnSpeed_Out_net[1];
              TurnSpeed_Out_net[1]=TurnSpeed_Out_net[0];
              TurnSpeed_Out_net[0]=TurnSpeed_Out_net_new;

              TurnSpeed_Out=TurnSpeed_Out_net[0]*0.4+TurnSpeed_Out_net[1]*0.3+TurnSpeed_Out_net[2]*0.2+TurnSpeed_Out_net[3]*0.1;
              TurnSpeed_Out=TurnSpeed_Out>300?300:TurnSpeed_Out;
              TurnSpeed_Out=TurnSpeed_Out<-300?-300:TurnSpeed_Out;
//           TurnSpeed_Out=(Diff_P * angle_value + Diff_D * derror -Servo_I*Balance_Dir_Gyro_Now/10 );//
//              else
//              TurnSpeed_Out=(Diff_P * angle_value- Servo_I*Balance_Dir_Gyro_Now/10 );



}



float PID_stop_speed_z (float speed)
{
    static float error_speed0 = 0, error_speed1 = 0, error_speed2 = 0;
    static float speed_Pid = 0;
    float add_speed = 0;
    error_speed0 = 0-speed;


//    error_speed0=error_speed0<-50?-50:error_speed0;
//    error_speed0=error_speed0>50?50:error_speed0;

//    add_speed = a0e * error_speed0 - a1e * error_speed1 + a2e * error_speed2;

    if(error_speed0<-3)
            speed_Pid=-980;
        else
            speed_Pid=0;


    error_speed2 = error_speed1;
    error_speed1 = error_speed0;

//    speed_Pid += add_speed;

    return speed_Pid;
}
float PID_stop_speed_y (float speed)
{
    static float error_speed0 = 0, error_speed1 = 0, error_speed2 = 0;
    static float speed_Pid = 0;
    float add_speed = 0;

    error_speed0 = 0-speed;


//    error_speed0=error_speed0<-50?-50:error_speed0;
//    error_speed0=error_speed0>50?50:error_speed0;

//    add_speed = a0e * error_speed0 - a1e * error_speed1 + a2e * error_speed2;
    if(error_speed0<-3)
        speed_Pid=-980;
    else
        speed_Pid=0;


    error_speed2 = error_speed1;
    error_speed1 = error_speed0;

//    speed_Pid += add_speed;

    return speed_Pid;
}

float PID_stop_speed_z_Buff (float speed)
{
    static float error_speed0 = 0, error_speed1 = 0, error_speed2 = 0;
    static float speed_Pid = 0;
    float add_speed = 0;
    if(PID_flag==1)
        {
            PID_flag=0;
            speed_Pid = 0;
            error_speed2=0;
            error_speed1=0;
        }
//    if(TurnSpeed_Out<0)
        error_speed0 = Speed_Aim_Buff//�����ٶ�
                -speed//ʵ���ٶ�
                +//ת�����
                TurnSpeed_Out*1.7;
//    else
//        error_speed0 = Speed_Aim_Buff-speed;

    error_speed0=error_speed0<-80?-80:error_speed0;
    error_speed0=error_speed0>80?80:error_speed0;
//        oled_show_int(64, 2, (int) error_speed0, 4);
//        oled_show_int(32, 2, (int) TurnSpeed_Out, 4);
//        oled_show_int(0, 2, (int) speed, 4);
    if(fabs(speed-Speed_Aim_Buff) < 0)
        add_speed = a0e * error_speed0 - a1e * error_speed1 + a2e * error_speed2;
    else
        add_speed = (Speed_P+Speed_D) * error_speed0 - a1e * error_speed1 + a2e * error_speed2;




    error_speed2 = error_speed1;
    error_speed1 = error_speed0;

    speed_Pid += add_speed;

    return speed_Pid;
}

float PID_stop_speed_y_Buff (float speed)
{
    static float error_speed0 = 0, error_speed1 = 0, error_speed2 = 0;
    static float speed_Pid = 0;
    float add_speed = 0;
    if(PID_flag==1)
        {
            PID_flag=0;
            speed_Pid = 0;
            error_speed2=0;
            error_speed1=0;
        }

//    if(TurnSpeed_Out>0)
    error_speed0 = Speed_Aim_Buff-speed-TurnSpeed_Out*1.7;
//        oled_show_int(64, 0, (int) error_speed0, 4);
//        oled_show_int(32, 0, (int) TurnSpeed_Out, 4);
//        oled_show_int(0, 0, (int) speed, 4);
//        else
//            error_speed0 = Speed_Aim_Buff-speed;

    error_speed0=error_speed0<-80?-80:error_speed0;
    error_speed0=error_speed0>80?80:error_speed0;
    if(fabs(speed-Speed_Aim_Buff) < 0)
        add_speed = a0e * error_speed0 - a1e * error_speed1 + a2e * error_speed2;
    else
        add_speed = (Speed_P+Speed_D) * error_speed0 - a1e * error_speed1 + a2e * error_speed2;


    error_speed2 = error_speed1;
    error_speed1 = error_speed0;

    speed_Pid += add_speed;

    return speed_Pid;
}

float PID_stop_speed_z_Buff1 (float speed)
{
    static float error_speed0 = 0, error_speed1 = 0, error_speed2 = 0;
    static float speed_Pid = 0;
    float add_speed = 0;
    static float add_speed_old[4]={0,0,0,0};
    if(PID_flag==1)
    {
        PID_flag=0;
        speed_Pid = 0;
        error_speed2=0;
        error_speed1=0;
        add_speed_old[0]=0;
        add_speed_old[1]=0;
        add_speed_old[2]=0;
        add_speed_old[3]=0;
    }

//    if(TurnSpeed_Out<0)
    if(TurnSpeed_Out<0)
        error_speed0 = Speed_Aim_Buff-speed+TurnSpeed_Out;
    else
        error_speed0 = Speed_Aim_Buff-speed+TurnSpeed_Out/12;
//    else
//        error_speed0 = Speed_Aim_Buff-speed;

    error_speed0=error_speed0<-80?-80:error_speed0;
    error_speed0=error_speed0>80?80:error_speed0;
//        oled_show_int(64, 2, (int) error_speed0, 4);
//        oled_show_int(32, 2, (int) TurnSpeed_Out, 4);
//        oled_show_int(0, 2, (int) speed, 4);
    if(fabs(speed-Speed_Aim_Buff) < 0)
        add_speed = a0e * error_speed0 - a1e * error_speed1 + a2e * error_speed2;
    else
        add_speed= (Speed_P+Speed_D) * error_speed0 - a1e * error_speed1 + a2e * error_speed2;

//    add_speed_old[1]=add_speed_old[0];


//    add_speed=add_speed_old[0]*0.4+add_speed_old[1]*0.3+add_speed_old[2]*0.2+add_speed_old[3]*0.1;
//
//    add_speed_old[3]=add_speed_old[2];
//    add_speed_old[2]=add_speed_old[1];
//    add_speed_old[1]=add_speed_old[0];

    error_speed2 = error_speed1;
    error_speed1 = error_speed0;

    speed_Pid += add_speed;

    return speed_Pid;
}


//�ٶȻ�
float PID_stop_speed_y_Buff1 (float speed)
{
    static float error_speed0 = 0, error_speed1 = 0, error_speed2 = 0;
    static float speed_Pid = 0;
    float add_speed = 0;
    static float add_speed_old[4]={0,0,0,0};

    if(PID_flag==1)
    {
        PID_flag=0;
        speed_Pid = 0;
        error_speed2=0;
        error_speed1=0;
        add_speed_old[0]=0;
        add_speed_old[1]=0;
        add_speed_old[2]=0;
        add_speed_old[3]=0;

    }

//    if(TurnSpeed_Out>0)
    if(TurnSpeed_Out>0)
        error_speed0 = Speed_Aim_Buff-speed-TurnSpeed_Out;
    else
        error_speed0 = Speed_Aim_Buff-speed-TurnSpeed_Out/12;
//        error_speed0 = Speed_Aim_Buff-speed-TurnSpeed_Out;
//        oled_show_int(64, 0, (int) error_speed0, 4);
//        oled_show_int(32, 0, (int) TurnSpeed_Out, 4);
//        oled_show_int(0, 0, (int) speed, 4);
//        else
//            error_speed0 = Speed_Aim_Buff-speed;

    error_speed0=error_speed0<-80?-80:error_speed0;
    error_speed0=error_speed0>80?80:error_speed0;
    //���ַ���
    if(fabs(speed-Speed_Aim_Buff) < 0)
        add_speed = a0e * error_speed0 - a1e * error_speed1 + a2e * error_speed2;
    else
        add_speed= (Speed_P+Speed_D) * error_speed0 - a1e * error_speed1 + a2e * error_speed2;



//    add_speed=add_speed_old[0]*0.4+add_speed_old[1]*0.3+add_speed_old[2]*0.2+add_speed_old[3]*0.1;

//    add_speed_old[3]=add_speed_old[2];
//    add_speed_old[2]=add_speed_old[1];
//    add_speed_old[1]=add_speed_old[0];

    error_speed2 = error_speed1;
    error_speed1 = error_speed0;

    //�������
    speed_Pid += add_speed;

    return speed_Pid;
}

/* 2023.8.31
 * ֱ�����ٶȻ�
 * PD����
 *  */
PID ZL_jiaosudu;
void ZL_PID_jiaosudu(float qiwang,float fankui)
{
    static float error;
    error = qiwang - fankui;
//    ZL_jiaosudu.integral += error;
    ZL_jiaosudu.output = ZL_jiaosudu.q1 * error
                    + ZL_jiaosudu.q3 * (error - ZL_jiaosudu.last_error);

    ZL_jiaosudu.last_error = error;
}


/* ���ٶ�����ʽ����
 * PI����
 * */
void PID_Increase(float qiwang,float fankui)
{
    static float PrevError = 0, LastError = 0, LastData = 0;
    //��ǰ������Ϊ�Ĵ���������ֻ���������ͺ��ַ��ͱ�������������ٶ�
    float iError,   //��ǰ���
        Increase;   //���ó���ʵ������

    iError = qiwang - fankui;   // ���㵱ǰ���

    Increase =  ZL_jiaosudu.q1 * (iError - LastError)
              + ZL_jiaosudu.q2 * iError
              + ZL_jiaosudu.q3 * (iError - 2 * LastError + PrevError);

    PrevError = LastError;  // ����ǰ�����
    LastError = iError;           // �����ϴ����
    LastData  = fankui;          // �����ϴ�����
    ZL_jiaosudu.output += Increase;
    if(ZL_jiaosudu.output>500)
        ZL_jiaosudu.output = 500;
    if(ZL_jiaosudu.output<-500)
        ZL_jiaosudu.output = -500;
}

/* 2023.8.31
 * ֱ�����ٶȻ�
 * P����
 *  */
PID ZL_jiaodu;
void ZL_PID_jiaodu(float qiwang, float fankui)
{
    static float error;
    error =qiwang - fankui;
//    ZL_jiaodu.integral += error;
    ZL_jiaodu.output = ZL_jiaodu.q1 * error + ZL_jiaodu.q3 * ((0.8 * error + 0.2 * ZL_jiaodu.last_error)  - ZL_jiaodu.last_error);

    ZL_jiaodu.last_error = error;

    ZL_jiaosudu.setpoint  =  ZL_jiaodu.output;
}

/* 2023.8.31
 * ֱ���ٶȻ�
 * ����PID
 *  */
PID ZL_sudu;
void ZL_speed_L(float qiwang,float shiji)
{
    static float error_speed0 = 0, error_speed1 = 0, error_speed2 = 0;
    float out=0;
    error_speed0 = qiwang - shiji;

    out =  ZL_sudu.q1 * (error_speed0 - error_speed1)
              + ZL_sudu.q2 * error_speed0
              + ZL_sudu.q3 * (error_speed0 - 2 * error_speed1 + error_speed2);
    error_speed2 = error_speed1;
    error_speed1 = error_speed0;

    seep_L_out += out;

}

//������
void ZL_speed_R(float qiwang,float shiji)
{
    static float error_speed0 = 0, error_speed1 = 0, error_speed2 = 0;
    float out=0;
    error_speed0 = qiwang + shiji;

    out =  ZL_sudu.q1 * (error_speed0 - error_speed1)
              + ZL_sudu.q2 * error_speed0
              + ZL_sudu.q3 * (error_speed0 - 2 * error_speed1 + error_speed2);
    error_speed2 = error_speed1;
    error_speed1 = error_speed0;

    seep_R_out += out;

}

void zLpidint(void)
{
    ZL_jiaosudu.setpoint = 0;
    ZL_jiaosudu.integral = 0;
    ZL_jiaosudu.last_error = 0;
    ZL_jiaosudu.output = 0;

    ZL_jiaodu.integral = 0;
    ZL_jiaodu.last_error = 0;
//    ZL_jiaodu.output = 0;
    ZL_jiaodu.setpoint = 0;
}

float Balance_P,Balance_I,Balance_D;

/**********�ٶȻ�PID����*****************/
float Speed_Control(void)
{
    float SpeedSum = Encoder_RawSpeedSum;
    float SpeedError;

    SpeedError = Speed_Aim - SpeedSum;

    static float SpeedOut = 0;
    static float LastError = 0;

    if(Speed_RunOutControlNum >= 100)
        Speed_RunOutControlNum = 0;
    SpeedOut= Speed_P*SpeedError + Speed_D*(SpeedError-LastError);
    LastError = SpeedError;
    Speed_OutAngleNew = SpeedOut;
    return SpeedOut;
}

/**********�ٶ�ƽ���������*****************/
void Speed_SmoothOut(void){
    Speed_RunOutControlNum++;
    Speed_OutAngle = (Speed_OutAngleNew - speed_LastAngle)*1/10.0+speed_LastAngle;
    Speed_ErrorAccumulated +=(Speed_OutAngleNew - speed_LastAngle);

    if(Speed_OutAngle >= speed_LastAngle)
      Speed_OutAngle = ((Speed_OutAngle - speed_LastAngle)>0.025?(speed_LastAngle+0.025):Speed_OutAngle);
    else
      Speed_OutAngle = ((Speed_OutAngle - speed_LastAngle)<-0.025?(speed_LastAngle-0.025):Speed_OutAngle);
    Speed_OutAngle +=(Speed_ErrorAccumulated*ErrorAccumulated_I);
    speed_LastAngle = Speed_OutAngle;
}

/**********�ǶȻ�P����*****************/
void Angle_Control(void)
{

    static float Change_SpeedOut = 0;
    static float ThreeTyre_SpeedErrorLast=0,ThreeTyre_SpeedErrorRateLast=0,ThreeTyre_SpeedSumLast=0, ThreeTyre_SpeedOutLast=0;
    float ThreeTyre_SpeedErrorRate, ThreeTyre_SpeedSum = 0, ThreeTyre_SpeedError = 0;
    float ThreeTyreSpeed_P=75, ThreeTyreSpeed_I=0.8, ThreeTyreSpeed_D =40;
    float Speed_RealAngle=0;

//             if(Ramp_Change==0)
//            {

    RealAngle = AngleAim - Att_Angle.rol;
//                if(Balance_Test==1 || Balance_Test==3)
//                 Speed_OutAngle=0;

    if(Speed_OutAngle>1.0)Speed_OutAngle=1.0;
    if(Speed_OutAngle<-0.5)Speed_OutAngle=-0.5;

    Speed_RealAngle=RealAngle+Speed_OutAngle*(!Ramp_add_time);
    if(Speed_RealAngle>6.0)Speed_RealAngle=6.0;
    if(Speed_RealAngle<-10.0)Speed_RealAngle=-10.0;

    Balance_Speed_Out = (int)(Balance_P * Speed_RealAngle - Balance_D * imugyro_y);
//            }
//            else
//            {
//                 if(Balance_Mode==0){//�޷�����
//                        RealAngle = Balance_VER - Balance_Bal_AG_Angle;
//                        Balance_Speed_Out = (int)(Balance_P * RealAngle- Balance_D * SystemAttitudeRate.Pitch);
//                     //����ʱ��һ����޷�
//                        if(Ramp_time>=Half_Ramp_Time&&Encoder_RawSpeedSum>100&&Ramp_Flag>1){
//
//                            if(Balance_Speed_Out>250)Balance_Speed_Out=250;
//                            if(Encoder_RawSpeedSum>300){
//                                    if(Balance_Speed_Out>200)Balance_Speed_Out=200;
//                            }
//                            else if(Encoder_RawSpeedSum>350){
//                                    if(Balance_Speed_Out>180)Balance_Speed_Out=180;
//                            }
//                        }
//                        //����ȫ�����ֵ�޷�����ֹ̧��
//                        if(Encoder_RawSpeedSum>500&&Ramp_Flag>1)
//                        {
//                            if(Balance_Speed_Out>450)Balance_Speed_Out=450;
//                        }
//
//                     }
//                else
//                {//�ٶȻ�����
//                        if(Ramp_time>=(Half_Ramp_Time))
//                        {
//                             ThreeTyre_SpeedSum = Encoder_RawSpeedSum ;
//                             ThreeTyre_SpeedError =((Ramp_DownSpeed - ThreeTyre_SpeedSum)>35?35:(Ramp_DownSpeed - ThreeTyre_SpeedSum))<-35?-35:((Ramp_DownSpeed - ThreeTyre_SpeedSum)>35?35:(Ramp_DownSpeed - ThreeTyre_SpeedSum));
//
//                             ThreeTyre_SpeedErrorRate=ThreeTyre_SpeedError - ThreeTyre_SpeedErrorLast;
//                             Change_SpeedOut+=ThreeTyre_SpeedErrorRate * ThreeTyreSpeed_P + ThreeTyre_SpeedError * ThreeTyreSpeed_I +(ThreeTyre_SpeedErrorRate-ThreeTyre_SpeedErrorRateLast) * ThreeTyreSpeed_D;
//                             if(Change_SpeedOut>20000)
//                                 Change_SpeedOut=20000;
//                             ThreeTyre_SpeedErrorLast= ThreeTyre_SpeedError;//
//                             ThreeTyre_SpeedErrorRateLast=ThreeTyre_SpeedErrorRate;
//                             ThreeTyre_SpeedOutLast=Change_SpeedOut;
//                             ThreeTyre_SpeedSumLast=ThreeTyre_SpeedSum;
//                             Balance_Speed_Out =(int)Change_SpeedOut;
//                        }
//                        else
//                        {
//                             RealAngle = Balance_VER - Balance_Bal_AG_Angle;
//                             Balance_Speed_Out = (int)(Balance_P * RealAngle - Balance_D * SystemAttitudeRate.Pitch);
//                        }
//                }
//         }
}

/**********ת��PID����*****************/
float Quadratic_Encoder_RawSpeedSum = 0;
void ZLTurn_PID(void)
{
    static int Turn_Target = 0,Turn_Target_last = 0;
    float Kp = 0,Kd = 0;

    Turn_Target = angle_value ;

    if(Encoder_RawSpeedSum<150)
    {
        Kp = Diff_P * ((1000.0+Encoder_RawSpeedSum)/1300.0);
        Kd = Diff_D / ((1000.0+Encoder_RawSpeedSum)/1200.0);
    }
    else if(Encoder_RawSpeedSum<250)
    {
        Kp = Diff_P * ((1000.0+Encoder_RawSpeedSum)/1200.0);
        Kd = Diff_D / ((1000.0+Encoder_RawSpeedSum)/1100.0);
    }
    else if(Encoder_RawSpeedSum<450)
    {
        Kp = Diff_P * ((1000.0+Encoder_RawSpeedSum)/1050.0); //1080
        Kd = Diff_D / ((1000.0+Encoder_RawSpeedSum)/1000.0);
    }
    else if(Encoder_RawSpeedSum<550)
    {
        Kp = Diff_P * ((1000.0+Encoder_RawSpeedSum)/850.0);  //900
        Kd = Diff_D / ((1000.0+Encoder_RawSpeedSum)/850.0);
    }
    else if(Encoder_RawSpeedSum<600)
    {
        Kp = Diff_P * ((1000.0+Encoder_RawSpeedSum)/800.0);
        Kd = Diff_D / ((1000.0+Encoder_RawSpeedSum)/800.0);
    }
    else
    {
        Kp = Diff_P * ((1000.0+Encoder_RawSpeedSum)/700.0);
        Kd = Diff_D / ((1000.0+Encoder_RawSpeedSum)/700.0);
    }

//               if((abs(Turn_Target) - abs(Turn_Target_last)) > 37&&AddSpeedCrazy_flag==1) //�������
//      {
//        Kp = Kp * 1.55;//1.48
//        Kd = Kd * 1.11;//1.11
//      }
//      else if((abs(Turn_Target_last) - abs(Turn_Target)) > -40&&AddSpeedCrazy_flag==1)  //�������
//      {
//        Kp = Kp * 1.11;
//        Kd = Kd * 1.45;
//      }

    Quadratic_Encoder_RawSpeedSum=(Encoder_RawSpeedSum*Encoder_RawSpeedSum/450000.0) + 0.92;
    Quadratic_Encoder_RawSpeedSum=Quadratic_Encoder_RawSpeedSum>2.3?2.3:Quadratic_Encoder_RawSpeedSum;
    TurnSpeed_Out=((Kp * angle_value + Kd * angle_value_Error)- DIFFrate_D * SystemAttitudeRate.Yaw)*Quadratic_Encoder_RawSpeedSum;
    TurnSpeed_Out=TurnSpeed_Out>300?300:TurnSpeed_Out;
    TurnSpeed_Out=TurnSpeed_Out<-300?-300:TurnSpeed_Out;

}





/* @�������ƣ�void PID_CascadeCalc_ZL(float suduqiwang,float L_encoder,float R_encoder,float shiji_roll,float shiji_gyro_y)
 * @�������ã�ֱ��������PID
 * @������float suduqiwang�����ٶȻ�������
 * @������float L_encoder�������ֱ�������ֵ
 * @������float R_encoder�������ֱ�������ֵ
 * @������float shiji_roll����ʵ�ʵĽǶ�
 * @������float shiji_gyro_y����ʵ�ʵĽ��ٶ�
 * @����ֵ����
 * @���ߣ�����
 * @ʱ�䣺2023��10��27��
 * */
void PID_CascadeCalc_ZL(float suduqiwang,float L_encoder,float R_encoder,float shiji_roll,float shiji_gyro_y)
{
    // Y_target_S�ٶȻ�����
    //Y_feedback_S���ӱ�����
    //Y_feedback_Rʵ�ʽǶ�
    //Y_feedback_Gʵ�ʽǼ��ٶ�
    static short Roll_4ms = 0;
    static short Roll_8ms = 0;
    static short Roll_20ms = 0;
    Roll_4ms++;
    Roll_8ms++;
    Roll_20ms++;

    if(Roll_20ms == 5)
    {
        Roll_20ms = 0;

        /* ��������� */
        Encoder_ValInit();
        /* ��ȡ���������� */
        Encoder_GetVal();
        /* λ��ʽ�������ٶȻ� */
        ZL_speed_L(suduqiwang,(L_encoder+R_encoder)*0.5);
        /* λ��ʽ�������ٶȻ� */
//        ZL_speed_R(suduqiwang,R_encoder);
        if(seep_L_out > 10) seep_L_out = 10;
        if(seep_L_out < -10) seep_L_out = -10;
//        if(seep_R_out > 10) seep_R_out = 10;
//        if(seep_R_out < -10) seep_R_out = -10;

    }

    if(Roll_8ms == 2)
    {
        Roll_8ms = 0;
        ZL_PID_jiaodu(AngleAim+seep_L_out,shiji_roll);//����ǶȻ�
        /* ת���⻷��ƫ� */
        Turn_PID(1);
    }

    if(Roll_4ms == 1)
    {
        Roll_4ms = 0;
        ZL_PID_jiaosudu(ZL_jiaodu.output,shiji_gyro_y);//����ʽ������ٶȻ�
    }

//    pid->Y_output=pid->y_Speed.output;//�ٶȻ����⻷���
//    pid->Y_output=pid->y_Roll.output;//�ǶȻ��⻷���
//    pid->Y_output=pid->y_Gyro.output;//�ڻ�������Ǵ���PID�����
}

